#%%
from util import times
import matplotlib.pyplot as plt
import numpy as np
import matplotlib.animation as animation
#%%

def read_line_data(file_name):
    file = open(file_name)
    data = file.readlines()
    data1 = data
    for i in range(len(data)):
        data1[i] = [ [float(x) for x in i.strip('[').split(',')]for i in data[i].strip().split("]")[:-1]]
    return data1

def  scatter_line_Data(data):
    fig = plt.figure()
    ax = plt.subplot(111, aspect = 'equal')
    Xs = [i[0] for i in data[0]]
    Ys = [i[1] for i in data[0]]
    line, = ax.plot(Xs, Ys)

    plt.xlim(-2, 2)
    plt.ylim(-2, 2)
    start = 1

    def init():
        line.set_data([],[])

    def animate(k):
        Xs = [i[0] for i in data[start]]
        Ys = [i[1] for i in data[start]]
        line.set_data(Xs,Ys)
        start += 1
        return line,
    anim = animation.FuncAnimation(fig,animate,frames=len(data)-1,init_func=init,blit=True,interval=10)
    # for line in data:
    #     line = np.array(line)
    #     plt.scatter(line[:,0],line[:,1])
    #     plt.show()
    plt.show() 
global start
start = 1
def visualize(data):
    X = [i[0] for i in data[0]]
    Y = [i[1] for i in data[0]]

    fig = plt.figure()
    ax = plt.subplot(111, aspect = 'equal')
    line, = ax.plot(X, Y, 'ro')

    plt.xlim(-2, 2)
    plt.ylim(-2,2)

    def init():
        line.set_data([], [])
        return line,

    def init2():
        line.set_data([], [])
        return line
    
    def animate(aa):
        # simulator.evolve(0.01)
        global start
        X = [i[0] for i in data[start]]
        Y = [i[1] for i in data[start]]
        
        start += 1
        start = start % len(data)
        # if start == len(data):
        #     start = start -1
        line.set_data(X, Y)
        return line,

    anim = animation.FuncAnimation(fig,
                                   animate,
                                   frames=5,
                                   init_func = init,
                                   blit = True,
                                   interval = 100)
    # Writer = animation.writers['ffmpeg']
    # writer = Writer(fps=15, metadata=dict(artist='Me'), bitrate=1800)
    anim.save('localmap_test.gif', writer='imagemagick', fps=5)
    plt.show()


def test_visualize():
    particles = [Particle(0.3, 0.5, 1),
                 Particle(0.0, -0.5, -1),
                 Particle(-0.1, -0.4, 3)]
    simulator = ParticleSimulator(particles)
    visualize(simulator)

if __name__ == '__main__':
    file_name = "data/hg_local_costmaps_"+times+".txt"
    data = read_line_data(file_name)
    visualize(data)



